/*
 Copyright [2014] [Zelin Wu]
   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at
     http://www.apache.org/licenses/LICENSE-2.0
   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
 */
/*
The application code is for Project: Smart Helmet from IIT IPRO397.
Code contributers: Zelin Wu, Patrick Karina
*/
int GREEN = 2;
int RED = 3;
int BLUE = 4;
int x_temp;
int y_temp;
int z_temp;
int x_offset;
int y_offset;
int z_offset;
float increment= 1.33;//1.33bit/g is 6.5mV/1023,50g=1.33*50=66.5,90g=1.33*90=119.5
int yellow_warning=65;
int red_warning=130;
float xyz;
unsigned long time;
// the setup routine runs once when you press reset:
void setup() {                
  // initialize the digital pin as an output.
  Serial.begin(9600);
  pinMode(RED, OUTPUT);
  pinMode(GREEN,OUTPUT);
  pinMode(BLUE,OUTPUT);
  x_offset=analogRead(A1);
  y_offset=analogRead(A2);
  z_offset=analogRead(A3);
}

// the loop routine runs over and over again forever:
void loop() {
    x_temp=analogRead(A1);
    float x = (analogRead(A1)-x_offset)*increment;
    y_temp=analogRead(A2);
    float y = (analogRead(A2)-y_offset)*increment;
    z_temp=analogRead(A3);
    float z = (analogRead(A3)-z_offset)*increment;
    xyz=sqrt(x*x+y*y+z*z);
    Serial.println(xyz);
    delay(10);
    if(xyz>=yellow_warning && xyz<red_warning){
      YELLOW_ON();      
    }
    else if(xyz>=red_warning)
    {
      RED_ON(); 
    }  
}

void RED_ON(){
   int i=0;
   time=millis();
  for(i=0;i<20;i++)
 {
   digitalWrite(RED,HIGH);
   while((millis()-time)<=500)
   {
     Serial.println(calcXYZ());
     delay(10);
   }
   time=millis();
   digitalWrite(RED,LOW);
   while((millis()-time)<=500)
   {
     Serial.println(calcXYZ());
     delay(10);
   }  
   time=millis();
  } 
}


void BLUE_ON(){
   int i=0;
  for(i=0;i<25;i++)
 {
   digitalWrite(BLUE,HIGH);
   delay(500);
   digitalWrite(BLUE,LOW);
   delay(500);
 } 
}

void YELLOW_ON(){
    int i=0;
    time=millis();
  for(i=0;i<20;i++)
 {
   digitalWrite(RED,HIGH);
   digitalWrite(GREEN,HIGH);
   while((millis()-time)<=500){
     Serial.println(calcXYZ());
     delay(10);
   }
   time=millis();
   digitalWrite(RED,LOW);
   digitalWrite(GREEN,LOW);
   while((millis()-time)<=500){
     Serial.println(calcXYZ());
     delay(10);
   }
   time=millis();
 }  
}

float calcXYZ(){
    x_temp=analogRead(A1);
    float x = (analogRead(A1)-x_offset)*increment;
    y_temp=analogRead(A2);
    float y = (analogRead(A2)-y_offset)*increment;
    z_temp=analogRead(A3);
    float z = (analogRead(A3)-z_offset)*increment;
    xyz=sqrt(x*x+y*y+z*z);
    return xyz;
}

